#include "base16.h"

static uint8_t base16_encode_table[16] = {
	'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'
};

static uint8_t base16_decode_table[256] = {
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	  0,  1,  2,  3,  4,  5,  6,  7,  8,  9,255,255,255,255,255,255,
	255, 10, 11, 12, 13, 14, 15,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255, 10, 11, 12, 13, 14, 15,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255
};

uint32_t base16_encode(uint8_t *psrc, uint32_t src_len, uint8_t *pdst, uint32_t dst_len) {
    if (BASE16_ENCODE_LEN(src_len) > dst_len) {
        return 0;
    }
    dst_len = BASE16_ENCODE_LEN(src_len);

    for (; src_len > 0; src_len--) {
        *pdst = base16_encode_table[*psrc >> 4]; pdst++;
        *pdst = base16_encode_table[*psrc & 0x0F]; pdst++;
        psrc++;
    }
    return dst_len;
}

uint32_t base16_decode(uint8_t *psrc, uint32_t src_len, uint8_t *pdst, uint32_t dst_len) {
    register uint8_t m0, m1; //register
    if (src_len & 0x01) {
        return 0;
    }
    src_len = BASE16_DECODE_LEN(src_len);
    if (src_len > dst_len) {
        return 0;
    }
    dst_len = src_len;

    for (; src_len > 0; src_len--) {
        m0 = base16_decode_table[*psrc];
        if (m0 >= 16) {
            return 0;
        }
        psrc++;

        m1 = base16_decode_table[*psrc];
        if (m1 >= 16) {
            return 0;
        }
        psrc++;

        *pdst = (m0 << 4) | m1;
        pdst++;
    }
    return dst_len;
}
